#ifndef	_VOLUME_3D_HASH_H
#define _VOLUME_3D_HASH_H

#include <unordered_map>
#include <iostream>
#include <sstream>
#include <assert.h>

template<typename T>
class volume3DHash
{
public:
	volume3DHash(int z, int y, int x, T empty, int blockSize = 16);
	~volume3DHash();

	//bucket hash version, much faster, 10x
	void addVoxel(int z, int y, int x, T value);
	T getVoxel(int z, int y, int x);

	//status check
	unsigned int occupiedBlocks(); //occupied block

	//accessors
	int getDimX(){return _dimX;}
	int getDimY(){return _dimY;}
	int getDimZ(){return _dimZ;}
	int getBlockDimX(){return _spacialHashDimX;}
	int getBlockDimY(){return _spacialHashDimY;}
	int getBlockDimZ(){return _spacialHashDimZ;}
	int getBlockSize(){return _blockSize;}
	T* getBlockRegularGridData(int z, int y, int x);

	//convert to volume, debugging 
	void convertToRawVolume();
	std::string number2String(int num);
	bool writeToFile(const char* filename, const char* path );

private:
	//spacial hash divide space into blocks
	//typedef std::tr1::unordered_map<unsigned int,   hashBlock> spacialHash;
	//typedef std::tr1::unordered_map<unsigned int, T> subVolumeHash;
	//std::vector<subVolumeHash*> _subVolumeHashArray;
	std::vector<T*> _volSparseRaw;


	int _dimX, _dimY, _dimZ;
	int _spacialHashDimX,	 _spacialHashDimY, _spacialHashDimZ;
	T _emptyValue;
	int _blockSize;

	T* _data;

};

//constructor
template<typename T>
volume3DHash<T>::volume3DHash(int z, int y, int x, T empty, int blockSize)
:_dimX(x),
 _dimY(y),
 _dimZ(z),
 _emptyValue(empty),
 _blockSize(blockSize),
 _data(NULL)
{
	//two level hash version
	if(x%blockSize != 0)
		_spacialHashDimX = x/blockSize + 1;
	else
	   _spacialHashDimX = x/blockSize;

	if(y%blockSize != 0)
		_spacialHashDimY = y/blockSize + 1;
	else
		_spacialHashDimY = y/blockSize;

	if(z%blockSize != 0)
		_spacialHashDimZ = z/blockSize + 1;
	else
		_spacialHashDimZ = z/blockSize;

	printf("init Vector %d %d %d\n", _spacialHashDimZ, _spacialHashDimY, _spacialHashDimX);
	for(int k=0; k<_spacialHashDimZ; k++)
		for(int j=0; j<_spacialHashDimY; j++)
			for(int i=0; i<_spacialHashDimX; i++)//this order matters
			{
				//subVolumeHash *volHash = NULL;
				//_subVolumeHashArray.push_back(volHash);
				_volSparseRaw.push_back(NULL);
			}
	printf("done init Vector\n");
	


}
template<typename T>
volume3DHash<T>::~volume3DHash()
{
	unsigned int size = _volSparseRaw.size();
	for(unsigned int i=0; i<size; i++)
	{
		if(_volSparseRaw[i] != NULL)
			delete _volSparseRaw[i];
	}

	if(_data != NULL)
		free(_data);

}

template<typename T>
void volume3DHash<T>::addVoxel(int z, int y, int x, T value)//0 to N-1
{
	//two level hash version
	int spacialHashIndexX = x/_blockSize;
	int spacialHashIndexY = y/_blockSize;
	int spacialHashIndexZ = z/_blockSize;
	//printf("xyz:%d %d %d \\\\", x, y, z);
	//printf("sXYZ:%d, %d, %d \\\\",spacialHashIndexX, spacialHashIndexY, spacialHashIndexZ);

	int subVolumeIndexX = x%_blockSize;// - spacialHashIndexX*_blockSize;
	int subVolumeIndexY = y%_blockSize;// - spacialHashIndexY*_blockSize;
	int subVolumeIndexZ = z%_blockSize;// - spacialHashIndexZ*_blockSize;
	//printf("%d %d %d  ",subVolumeIndexX, subVolumeIndexY, subVolumeIndexZ);

	//find the right index
	int index = spacialHashIndexZ*(_spacialHashDimX*_spacialHashDimY)
					+ spacialHashIndexY*_spacialHashDimX + spacialHashIndexX;

	int indexSub = subVolumeIndexZ*(_blockSize*_blockSize) 
					+ subVolumeIndexY*_blockSize + subVolumeIndexX;

	//if(_subVolumeHashArray[index] == NULL)
	//	_subVolumeHashArray[index] = new subVolumeHash;
	//_subVolumeHashArray[index]->insert(subVolumeHash::value_type(indexSub, value));

	if(_volSparseRaw[index] == NULL)
		_volSparseRaw[index] = (T*)malloc(_blockSize*_blockSize*_blockSize*sizeof(T));
	//if(indexSub>=_blockSize*_blockSize*_blockSize)
	//{
	//	printf("indexSub error! blockSize: %d\n", _blockSize);
	//	exit(0);
	//}
	_volSparseRaw[index][indexSub] = value;

}
template<typename T>
T volume3DHash<T>::getVoxel(int z, int y, int x)//0 - z
{
	int spacialHashIndexX = x/_blockSize;
	int spacialHashIndexY = y/_blockSize;
	int spacialHashIndexZ = z/_blockSize;

	int subVolumeIndexX = x%_blockSize;// - spacialHashIndexX*_blockSize;
	int subVolumeIndexY = y%_blockSize;// - spacialHashIndexY*_blockSize;
	int subVolumeIndexZ = z%_blockSize;// - spacialHashIndexZ*_blockSize;
	//printf("xyz:%d %d %d \\\\", x, y, z);
	//printf("sXYZ:%d, %d, %d \\\\",spacialHashIndexX, spacialHashIndexY, spacialHashIndexZ);

	//find the right index
	int index = spacialHashIndexZ*(_spacialHashDimX*_spacialHashDimY)
		+ spacialHashIndexY*_spacialHashDimX + spacialHashIndexX;

	int indexSub = subVolumeIndexZ*(_blockSize*_blockSize) 
		+ subVolumeIndexY*_blockSize + subVolumeIndexX;

	//if(_subVolumeHashArray[index] != NULL)
	//	if(_subVolumeHashArray[index]->find(indexSub) != _subVolumeHashArray[index]->end())
	//		return _subVolumeHashArray[index]->operator[](indexSub);
	//if(index>=_spacialHashDimX*_spacialHashDimY*_spacialHashDimZ)
	//	printf("index error@@@@@ index:%d %d %d\n", spacialHashIndexZ, spacialHashIndexY, spacialHashIndexX);
	if(_volSparseRaw[index] != NULL)
	{
		//if(indexSub>=_blockSize*_blockSize*_blockSize)
		//	printf("indexSub\n");
		return _volSparseRaw[index][indexSub];
	}

	return _emptyValue;
}

//////////////////status check
template<typename T>
unsigned int volume3DHash<T>::occupiedBlocks() //occupied block
{
	unsigned int count = 0;
	unsigned int size = _volSparseRaw.size();
	for(unsigned int i=0; i<size; i++)
		if(_volSparseRaw[i] != NULL)
			count++;
	return count;

}

template <typename T>
T* volume3DHash<T>::getBlockRegularGridData(int z, int y, int x)
{
	unsigned int index = z*(_spacialHashDimX*_spacialHashDimY)
		+ y*_spacialHashDimX + x;
	if(_volSparseRaw[index] == NULL)
		return NULL;
	
	//if(_subVolumeHashArray[index] == NULL)
	//	return NULL;

	T* blockBuffer = (T*)malloc(sizeof(T)*_blockSize*_blockSize*_blockSize);
	memset(blockBuffer,_emptyValue,sizeof(T)*_blockSize*_blockSize*_blockSize);
	for(int i=0; i<_blockSize; i++)
		for(int j=0; j<_blockSize; j++)
			for(int k=0; k<_blockSize; k++)
			{
				long bIndex = i*_blockSize*_blockSize + j*_blockSize + k;

				blockBuffer[bIndex] = getVoxel(z*_blockSize + i,y*_blockSize + j,x*_blockSize + k);
			}
	return blockBuffer;
}


/////////////////////////////////////////////////////////
//////////

template<typename T>
void volume3DHash<T>::convertToRawVolume()
{
	_data = (T*)malloc(sizeof(T)*_dimX*_dimY*_dimZ);
	memset(_data,0,sizeof(T)*_dimX*_dimY*_dimZ);
	printf("memset %d %d %d", _dimX, _dimY, _dimZ);
	//T value = _emptyValue;
	//for(int k=0; k<_spacialHashDimZ; k++)
	//	for(int j=0; j<_spacialHashDimY; j++)
	//		for(int i=0; i<_spacialHashDimX; i++)//this order matters
	//		{
	//			//find the right index
	//				int index = k*(_spacialHashDimX*_spacialHashDimY)
	//								+ j*_spacialHashDimX + i;

	//				if(_subVolumeHashArray[index] != NULL)
	//				{
	//						for(int kk=0; kk<_blockSize; kk++)
	//							for(int jj=0; jj<_blockSize; jj++)
	//								for(int ii=0; ii<_blockSize; ii++)//this order matters
	//								{

	//									int indexSub = kk*(_blockSize*_blockSize) 
	//										+ jj*_blockSize + ii;

	//									if(_subVolumeHashArray[index]->find(indexSub) != _subVolumeHashArray[index]->end())
	//									{
	//										long gIndex =	(k*_spacialHashDimZ+kk)*_dimX*_dimY 
	//											+ (j*_spacialHashDimY+jj)*_dimX + i*_spacialHashDimX+ii;
	//										value =  _subVolumeHashArray[index]->operator[](indexSub);
	//										_data[gIndex] = value;
	//									}
	//								}					

	//				}

	//		}

	for(int i=0; i<_dimZ; i++)
		{
			for(int j=0; j<_dimY; j++)
				for(int k=0; k<_dimX; k++)
				{
					long gIndex = i*_dimY*_dimX + j*_dimX + k;
					//if(gIndex>= _dimZ*_dimY*_dimX)
					//	printf("gIndex error\n");
					_data[gIndex] = getVoxel(i,j,k);
				}
		}


}
 template<typename T>
std::string  volume3DHash<T>::number2String(int num)
{
	std::ostringstream stm;
	stm << num;										 
	return std::string(stm.str());
}

template<typename T>
bool volume3DHash<T>::writeToFile(const char* filename, const char* path)
{
	FILE *pFile; 

	std::string fpath = std::string(path);
	std::string fname=std::string(filename);
	fname = fpath + fname;
	fname += std::string("-zyx-");
	fname += (number2String(_dimZ)+std::string("X")+number2String(_dimY)+std::string("X")+number2String(_dimX));
	fname += std::string(".raw");

	pFile= fopen(fname.c_str(), "wb");
	if (NULL == pFile) {
		cout<<"can't open file!"<<endl;
		return false;
	}

	int size = fwrite((void*)_data, sizeof(T), _dimX*_dimY*_dimZ, pFile);
	if (size !=  _dimX*_dimY*_dimZ)
	{
		cout << "file Write Error"<< endl;
		return false;
	}
	cout <<"write:"<<fname.c_str()<< endl;
	return true;
}


#endif 